/**
* @file RstDecoder.cpp in InsCubeDecoder
* @author rebeater
* @comment
* Create on 3/14/22 8:53 PM
* @version 1.0
**/


#include "RstHandle.h"
namespace InsCube{

InsCube::RstHandle::RstHandle() : HandleBase("INS-Cube") {
}
bool InsCube::RstHandle::ParserNav(uint8_t *data, NavOutput &output) const {
  if (CrcCheckSum((uint8_t *)data, sizeof(NavPva)  + 18)==0) {
    output.week = *(short *)(data + 2);
    output.gpst = *(double *)(data + 6);
    NavPva pva = *(NavPva * )(data + 14);
    output.lat = pva.lat;
    output.lon = pva.lon;
    output.height = pva.height;
    output.vn[0] = pva.vn[0];
    output.vn[1] = pva.vn[1];
    output.vn[2] = pva.vn[2];
    output.atti[0] = pva.atti[0];
    output.atti[1] = pva.atti[1];
    output.atti[2] = pva.atti[2];
    output.info.mode = pva.info.mode;
    output.info.sensors = pva.info.sensors;
    return true;
  }
  return false;

}
#define CRC32_POLY 0x04C11DB7U
#define CRC32_START 0xFFFFFFFFU
#ifndef HARDWARE_CRC
#define HARDWARE_CRC  0
#endif
uint32_t InsCube::RstHandle::CrcCheckSum(const uint8_t *indata, int num_bytes) const {
  #if HARDWARE_CRC == 1
  return 0xfff;
  /*在STM32中通过硬件硬件实现 */
#else
  /*软件校验*/
  uint32_t crc = CRC32_START;
  const uint8_t *p = indata;
  for (int i = 0; i < num_bytes; i++) {
    crc = crc ^ (*p++ << 24U);
    for (int bit = 0; bit < 8; bit++) {
      if (crc & (1LU << 31U)) crc = (crc << 1U) ^ CRC32_POLY;
      else crc = (crc << 1U);
    }
  }
#endif
  return crc;
}
}